#include "PLOpenCVExtensionMethods.h"

#define ICV_HLINE_INTERPOLATED( ptr, xl, xr, begin_color, end_color, pix_size, img_type ) \
{                                                            \
	uchar* hline_ptr = (uchar*)(ptr) + (xl)*(pix_size);      \
	uchar* hline_max_ptr = (uchar*)(ptr) + (xr)*(pix_size);  \
	Scalar current_color( begin_color ); \
	Scalar color_inc = (end_color - begin_color) / ((xr - xl) > 0 ? (xr - xl) : 1.0);\
	double color[4];\
	\
	for (; hline_ptr <= hline_max_ptr; hline_ptr += (pix_size))\
	{														 \
		scalarToRawData(current_color, color, img_type, 0);  \
		int hline_j;                                         \
		for ( hline_j = 0; hline_j < (pix_size); hline_j++ ) \
		{                                                    \
			hline_ptr[hline_j] = ((uchar*)color)[hline_j];   \
		}                                                    \
		current_color += color_inc;							 \
	}                                                        \
}

namespace cv
{
	enum { XY_SHIFT = 16, XY_ONE = 1 << XY_SHIFT, DRAWING_STORAGE_BLOCK = (1<<12) - 256 };
};

/**
  *	Fills a convex polygon defined by some points. Every point has its own color.
  * For the pixels between the points the colors are linearly interpolated.
  *
  * @param img reference to the matrix representing the destination image for the polygon
  * @param pts const pointer to an array of Point objects defining the polygon
  * @param npts number of points in the polygon
  * @param colors const pointer to an array of Scalar objects representing the color information
  */
void cv::fillConvexPolyInterpolated( Mat& img, const Point* pts, int npts, const Scalar *colors )
{
	if( !pts || npts <= 0 )
		return;

	const int line_type = 8;

	struct
	{
		int idx, di;
		int x, dx, ye;
		Scalar current_color;
		Scalar color_inc;
	}
	edge[2];

	int i, y, imin = 0, left = 0, right = 1, x1, x2;
	int edges = npts;
	int xmin, xmax, ymin, ymax;
	uchar* ptr = img.data;
	Size size = img.size();
	int pix_size = (int)img.elemSize();
	Point p0;
	int delta1, delta2;

	delta1 = delta2 = XY_ONE >> 1;

	const Scalar *p0_color = 0;
	const Scalar *p1_color = 0;

	p0 = pts[npts - 1];
	p0.x <<= XY_SHIFT;
	p0.y <<= XY_SHIFT;

	p0_color = &colors[npts - 1];

	xmin = xmax = pts[0].x;
	ymin = ymax = pts[0].y;

	for( i = 0; i < npts; i++ )
	{
		Point p = pts[i];
		if( p.y < ymin )
		{
			ymin = p.y;
			imin = i;
		}

		ymax = std::max( ymax, p.y );
		xmax = std::max( xmax, p.x );
		xmin = MIN( xmin, p.x );

		p.x <<= XY_SHIFT;
		p.y <<= XY_SHIFT;

		Point pt0, pt1;
		pt0.x = p0.x >> XY_SHIFT;
		pt0.y = p0.y >> XY_SHIFT;
		pt1.x = p.x >> XY_SHIFT;
		pt1.y = p.y >> XY_SHIFT;
		p1_color = &colors[i];

		//Line( img, pt0, pt1, color, line_type );
		interpolatedLine( img, pt0, pt1, *p0_color, *p1_color );

		p0 = p;
		p0_color = &colors[i];
	}

	if( npts < 3 || xmax < 0 || ymax < 0 || xmin >= size.width || ymin >= size.height )
		return;

	ymax = MIN( ymax, size.height - 1 );
	edge[0].idx = edge[1].idx = imin;

	edge[0].ye = edge[1].ye = y = ymin;
	edge[0].di = 1;
	edge[1].di = npts - 1;

	ptr += img.step*y;

	do
	{
		if( line_type < CV_AA || y < ymax || y == ymin )
		{
			for( i = 0; i < 2; i++ )
			{
				if( y >= edge[i].ye )
				{
					int idx = edge[i].idx, di = edge[i].di;
					int start_index = idx;
					int xs = 0, xe, ye, ty = 0;

					for(;;)
					{
						ty = pts[idx].y;
						if( ty > y || edges == 0 )
							break;
						xs = pts[idx].x;
						start_index = idx;

						idx += di;
						idx -= ((idx < npts) - 1) & npts;   /* idx -= idx >= npts ? npts : 0 */

						edges--;
					}

					ye = ty;
					xs <<= XY_SHIFT;
					xe = pts[idx].x << XY_SHIFT;

					/* no more edges */
					if( y >= ye )
						return;

					edge[i].current_color = colors[start_index];
					edge[i].color_inc = (colors[idx] - colors[start_index]) / static_cast<double>( (ye - pts[start_index].y) );

					edge[i].ye = ye;
					edge[i].dx = ((xe - xs)*2 + (ye - y)) / (2 * (ye - y));
					edge[i].x = xs;
					edge[i].idx = idx;
				}
			}
		}

		if( edge[left].x > edge[right].x )
		{
			left ^= 1;
			right ^= 1;
		}

		x1 = edge[left].x;
		x2 = edge[right].x;

		if( y >= 0 )
		{
			int xx1 = (x1 + delta1) >> XY_SHIFT;
			int xx2 = (x2 + delta2) >> XY_SHIFT;

			if( xx2 >= 0 && xx1 < size.width )
			{
				if( xx1 < 0 )
					xx1 = 0;
				if( xx2 >= size.width )
					xx2 = size.width - 1;

				ICV_HLINE_INTERPOLATED( ptr, xx1, xx2, edge[left].current_color, edge[right].current_color, pix_size, img.type() );
			}
		}

		x1 += edge[left].dx;
		x2 += edge[right].dx;

		edge[left].x = x1;
		edge[right].x = x2;

		edge[left].current_color += edge[left].color_inc;
		edge[right].current_color += edge[right].color_inc;

		ptr += img.step;
	}
	while( ++y <= ymax );
}

/**
  *	Draws a line defined by two integer-points. Both points have their own color.
  * For the pixels between the points the colors are linearly interpolated.
  *
  * @param img reference to the matrix representing the destination image for the line
  * @param pt1 Point object representing the beginning of the line
  * @param pt2 Point object representing the end of the line
  * @param colorPt1 color of the first point
  * @param colorPt2 color of the second point
  * @param lineType 4 for a 4-connected line, 8 for a 8-connected line
  */
void cv::interpolatedLine( Mat &img, Point2i pt1, Point2i pt2,
	const Scalar &colorPt1, const Scalar &colorPt2, int lineType )
{
	assert( lineType == 4 || lineType == 8 );

 	int dx = pt2.x - pt1.x;
 	int dy = pt2.y - pt1.y;

	Point2i *begin = 0;
	Point2i *end = 0;

	const Scalar *start_color = 0;
	const Scalar *end_color = 0;
	
	Scalar current_color;
	Scalar color_inc;

	double buf_current_color[4];

	LineIterator iterator(img, pt1, pt2, lineType, true);
	int i, count = iterator.count;
	int pix_size = (int)img.elemSize();
	const uchar* color = 0;
	uchar* ptr = 0;

	if (dx == 0)
	{
		// Vertical line
		begin = &pt1;
		end = &pt2;
		start_color = &colorPt1;
		end_color = &colorPt2;
	}
	else
	{
		// Arbitrary line
		if (pt1.x < pt2.x)
		{
			begin = &pt1;
			end = &pt2;
			start_color = &colorPt1;
			end_color = &colorPt2;
		}
		else
		{
			begin = &pt2;
			end = &pt1;
			start_color = &colorPt2;
			end_color = &colorPt1;
		}
	}

	current_color = *start_color;

	if (count < 2)
		color_inc = Scalar( 0.0, 0.0, 0.0, 0.0 );
	else
	{
		color_inc = (*end_color - *start_color);

		color_inc = (*end_color - *start_color);
		color_inc[0] /= (count - 1);
		color_inc[1] /= (count - 1);
		color_inc[2] /= (count - 1);
		color_inc[3] /= (count - 1);
	}

	for (i = 0; i < count; i++, ++iterator)
	{
		scalarToRawData( current_color, buf_current_color, img.type(), 0 );
		color = (const uchar*)buf_current_color;

		ptr = *iterator;
		if( pix_size == 1 )
			ptr[0] = color[0];
		else if( pix_size == 3 )
		{
			ptr[0] = color[0];
			ptr[1] = color[1];
			ptr[2] = color[2];
		}
		else
			memcpy( *iterator, color, pix_size );

		current_color += color_inc;
	}
}

/**
  *	Draws a line defined by two double-points. Both points have their own color.
  * For the pixels between the points the colors are linearly interpolated.
  *
  * @param img reference to the matrix representing the destination image for the line
  * @param pt1 Point object representing the beginning of the line
  * @param pt2 Point object representing the end of the line
  * @param colorPt1 color of the first point
  * @param colorPt2 color of the second point
  * @param lineType 4 for a 4-connected line, 8 for a 8-connected line
  */
void cv::interpolatedLine( Mat &img, Point2d pt1, Point2d pt2,
	const Scalar &colorPt1, const Scalar &colorPt2, int lineType )
{
	interpolatedLine( img, Point2i( pt1 ), Point2i( pt2 ), colorPt1, colorPt2 );
}